/*
 * PR.h
 *
 *  Created on: Jun 10, 2024
 *      Author: Liangmiyi
 */

#ifndef MY_INC_PHY_PHY_PR_H_
#define MY_INC_PHY_PHY_PR_H_

#include <MY_INC/PHY/PHY_IIRFilter.h>
#include <math.h>
#include <stdint.h>
#define Harmonic_Order 3U // 谐波阶次   1、3、5、7次谐波

/****
 * @brief
 *
 */
typedef struct
{
    float Kp; // 比例增益
    float Kr; // 比例增益
    float wc; // 比例增益
    float w0; // 频率
    float ts; // 采样周期

    float output_of_feedback;            // 反馈输出
    float output_of_backward_integrator; // 后馈积分器输出
    float output_of_forward_integrator;  // 前馈积分器输出
    float input_of_forward_integrator;   // 前馈积分器输入
    float Output;                        // PR输出
    float error;                         // 误差

    float temp1;   // 模板参数1
    float temp2;   // 模板参数2
    float w0_2_ts; // w0^2 * ts
} PR_Typedef;

/**
 * @brief 普通PR算法初始化函数
 * @param PR PR结构体指针
 * @param Kp 比例增益
 * @param Kr 静态增益
 * @param w0 频率
 * @param ts 采样周期
 * @param wc 参数wc
 */
void PR_Calculate_Init(PR_Typedef *PR, float Kp, float Kr, float w0, float wc, float ts);

/**
 * @brief 普通PR算法计算函数
 * @param PR PR结构体指针
 * @param PR_Target 目标电压
 * @param PR_Actual 当前电压
 * @return PR算法输出值
 */
// 执行时间 103.69ns
float               PR_Calculate(PR_Typedef *PR, float PR_Target, float PR_Actual);
static inline float PR_Calculate_Inline(PR_Typedef *PR, float PR_Target, float PR_Actual)
{
    /*===================== PR 计算 =====================*/
    // 计算误差
    PR->error = PR_Target - PR_Actual;
    // 计算前馈积分器输入
    PR->input_of_forward_integrator = (PR->temp1 * PR->error) - PR->output_of_feedback;
    // 计算前馈积分器输出
    PR->output_of_forward_integrator += PR->ts * PR->input_of_forward_integrator;
    // 计算后馈积分器输出
    PR->output_of_backward_integrator += PR->output_of_forward_integrator * PR->w0_2_ts;
    // 计算反馈输出
    PR->output_of_feedback = PR->output_of_backward_integrator + PR->temp2 * PR->output_of_forward_integrator;
    // 计算最终输出
    PR->Output = (PR->output_of_forward_integrator + PR->Kp * PR->error);
    return PR->Output;
}
/**
 * @brief 三相PR算法结构体
 */
float               PR_Harmonic_Calculate(PR_Typedef *PR[], float PR_Target, float PR_Actual);
static inline float PR_Harmonic_Calculate_Inline(PR_Typedef *PR[], float PR_Target, float PR_Actual)
{
    /*===================== 单次 PR 计算 =====================*/

    float Out = 0.0f;
    // 计算误差
    float error = PR_Target - PR_Actual;
    for (uint8_t i = 0; i < Harmonic_Order; i++)
    {
        // 计算前馈积分器输入
        PR[i]->input_of_forward_integrator = (PR[i]->temp1 * error) - PR[i]->output_of_feedback;
        // 计算前馈积分器输出
        PR[i]->output_of_forward_integrator += PR[i]->ts * PR[i]->input_of_forward_integrator;
        // 计算后馈积分器输出
        PR[i]->output_of_backward_integrator += PR[i]->output_of_forward_integrator * PR[i]->w0_2_ts;
        // 计算反馈输出
        PR[i]->output_of_feedback =
            PR[i]->output_of_backward_integrator + PR[i]->temp2 * PR[i]->output_of_forward_integrator;
        // 计算最终输出
        PR[i]->Output = (PR[i]->output_of_forward_integrator + PR[i]->Kp * error);
        Out += PR[i]->Output;
    }
    return Out;
}
static inline float PMR_Calculate_Inline(PR_Typedef *PR[Harmonic_Order], float PR_Target, float PR_Actual)
{
    /*===================== 单次 PR 计算 =====================*/
    float Out = 0.0f;
    // 计算误差
    float error = PR_Target - PR_Actual;
    float Kp_Tmep = error * PR[0]->Kp;
    error = 2.0f * error * PR[0]->wc;
    for (uint8_t i = 0; i < Harmonic_Order; i++)
    {
        // 计算前馈积分器输入
        PR[i]->input_of_forward_integrator = (PR[i]->temp1 * error) - PR[i]->output_of_feedback;
        // 计算前馈积分器输出
        PR[i]->output_of_forward_integrator += PR[i]->ts * PR[i]->input_of_forward_integrator;
        // 计算后馈积分器输出
        PR[i]->output_of_backward_integrator += PR[i]->output_of_forward_integrator * PR[i]->w0_2_ts;
        // 计算反馈输出
        PR[i]->output_of_feedback =
            PR[i]->output_of_backward_integrator + PR[i]->temp2 * PR[i]->output_of_forward_integrator;
        // 计算最终输出
        PR[i]->Output = (PR[i]->output_of_forward_integrator);
        Out += PR[i]->Output;
    }
    return Out + Kp_Tmep;
}

/**
 * @brief 差分PR算法结构体
 */
typedef struct
{
    float Coeff_B0;
    float Coeff_B1;
    float Coeff_B2;
    float Coeff_A1;
    float Coeff_A2;

    float Xn_1; // 输入信号Xn-1
    float Xn_2; // 输入信号Xn-2
    float Yn_1; // 输出信号Yn-1
    float Yn_2; // 输出信号Yn-2

    float outmin; // 输出信号最小值
    float outmax; // 输出信号最大值

} PR_Diff_Typedef;

/**
 * @brief 差分PR算法初始化函数
 * @param PR 差分PR结构体指针
 * @param Kp 比例增益
 * @param Kr 静态增益
 * @param w0 频率
 * @param ts 采样周期
 * @param wc 参数wc
 */
void PR_Diff_Init(PR_Diff_Typedef *PR_Diff, float Kp, float Kr, float wo, float wc, float ts, float outmax,
                  float outmin);
void PR_Diff_Reset(PR_Diff_Typedef *PR_Diff);
/**
 * @brief 差分PR算法计算函数
 * @param PR 差分PR结构体指针
 * @param Target 目标电压
 * @param Actual 当前电压
 * @return PR算法输出值
 */
static inline float PRdiff_Calculate_Inline(PR_Diff_Typedef *PR_Diff, float Target, float Actual)
{
    /*===================== PR 计算 =====================*/
    float Xn = Target - Actual; // 计算误差
    float Yn = PR_Diff->Coeff_B0 * Xn + PR_Diff->Coeff_B1 * PR_Diff->Xn_1 + PR_Diff->Coeff_B2 * PR_Diff->Xn_2 -
               PR_Diff->Coeff_A1 * PR_Diff->Yn_1 - PR_Diff->Coeff_A2 * PR_Diff->Yn_2;
    if (Yn > PR_Diff->outmax)
        Yn = PR_Diff->outmax;
    else if (Yn < PR_Diff->outmin)
        Yn = PR_Diff->outmin;
    // 更新历史缓冲区
    PR_Diff->Xn_2 = PR_Diff->Xn_1; // Xn-2 = Xn-1
    PR_Diff->Xn_1 = Xn;            // Xn-1 = Xn
    PR_Diff->Yn_2 = PR_Diff->Yn_1; // Yn-2 = Yn-1
    PR_Diff->Yn_1 = Yn;            // Yn-1 = Yn

    return Yn; // 返回当前输出信号
}
static inline float PMRdiff_Calculate_Inline(PR_Diff_Typedef *PR_Diff[], float Target, float Actual)
{
    /*===================== 单次 PR 计算 =====================*/
    float Out = 0.0f;
    float Xn = Target - Actual; // 计算误差
    float Yn = 0.0f;
    for (uint8_t i = 0; i < Harmonic_Order; i++)
    {
        // 现在 PR_Diff[i] 是一个结构体，直接使用点运算符 '.' 访问其成员
        Yn = PR_Diff[i]->Coeff_B0 * Xn + PR_Diff[i]->Coeff_B1 * PR_Diff[i]->Xn_1 +
             PR_Diff[i]->Coeff_B2 * PR_Diff[i]->Xn_2 - PR_Diff[i]->Coeff_A1 * PR_Diff[i]->Yn_1 -
             PR_Diff[i]->Coeff_A2 * PR_Diff[i]->Yn_2;
        if (Yn > PR_Diff[i]->outmax)
            Yn = PR_Diff[i]->outmax;
        else if (Yn < PR_Diff[i]->outmin)
            Yn = PR_Diff[i]->outmin;
        PR_Diff[i]->Xn_2 = PR_Diff[i]->Xn_1; // Xn-2 = Xn-1
        PR_Diff[i]->Xn_1 = Xn;               // Xn-1 = Xn
        PR_Diff[i]->Yn_2 = PR_Diff[i]->Yn_1; // Yn-2 = Yn-1
        PR_Diff[i]->Yn_1 = Yn;               // Yn-1 = Yn
        Out += Yn;                           // 返回当前输出信号
    }
    return Out;
}
/**
 * @brief 三相PR算法结构体
 */
typedef struct
{
    float Kp; // 比例增益
    float ts; // 采样周期

    float output_of_feedback[3];            // 反馈输出
    float output_of_backward_integrator[3]; // 后馈积分器输出
    float output_of_forward_integrator[3];  // 前馈积分器输出
    float input_of_forward_integrator[3];   // 前馈积分器输入
    float Output[3];                        // PR输出
    float error[3];                         // 误差

    float temp1;   // 模板参数1
    float temp2;   // 模板参数2
    float w0_2_ts; // w0^2 * ts
} ThreePhase_PR_Typedef;

/**
 * @brief 三相PR算法初始化函数
 * @param PR 三相PR结构体指针
 * @param Kp 比例增益
 * @param Kr 静态增益
 * @param w0 频率
 * @param ts 采样周期
 * @param wc 参数wc
 */
void ThreePhase_PR_Calculate_Init(ThreePhase_PR_Typedef *PR, float Kp, float Kr, float w0, float ts, float wc);

/**
 * @brief 三相PR算法计算函数
 * @param PR 三相PR结构体指针
 * @param PR_Target 目标电压数组
 * @param PR_Actual 当前电压数组
 */
void ThreePhase_PR_Calculate(ThreePhase_PR_Typedef *PR, float PR_Target[3], float PR_Actual[3]);
// static inline void ThreePhase_PR_Calculate(ThreePhase_PR_Typedef *PR, float PR_Target[3], float PR_Actual[3])
// {
//     // A 相
//     PR->error[0] = PR_Target[0] - PR_Actual[0];
//     PR->input_of_forward_integrator[0] = PR->temp1 * PR->error[0] - PR->output_of_feedback[0];
//     PR->output_of_forward_integrator[0] += PR->ts * PR->input_of_forward_integrator[0];
//     PR->output_of_backward_integrator[0] += PR->output_of_forward_integrator[0] * PR->w0_2_ts;
//     PR->output_of_feedback[0] = PR->output_of_backward_integrator[0] + PR->temp2 *
//     PR->output_of_forward_integrator[0]; PR->Output[0] = PR->output_of_forward_integrator[0] + PR->Kp * PR->error[0];

//     // B 相
//     PR->error[1] = PR_Target[1] - PR_Actual[1];
//     PR->input_of_forward_integrator[1] = PR->temp1 * PR->error[1] - PR->output_of_feedback[1];
//     PR->output_of_forward_integrator[1] += PR->ts * PR->input_of_forward_integrator[1];
//     PR->output_of_backward_integrator[1] += PR->output_of_forward_integrator[1] * PR->w0_2_ts;
//     PR->output_of_feedback[1] = PR->output_of_backward_integrator[1] + PR->temp2 *
//     PR->output_of_forward_integrator[1]; PR->Output[1] = PR->output_of_forward_integrator[1] + PR->Kp * PR->error[1];

//     // C 相
//     PR->error[2] = PR_Target[2] - PR_Actual[2];
//     PR->input_of_forward_integrator[2] = PR->temp1 * PR->error[2] - PR->output_of_feedback[2];
//     PR->output_of_forward_integrator[2] += PR->ts * PR->input_of_forward_integrator[2];
//     PR->output_of_backward_integrator[2] += PR->output_of_forward_integrator[2] * PR->w0_2_ts;
//     PR->output_of_feedback[2] = PR->output_of_backward_integrator[2] + PR->temp2 *
//     PR->output_of_forward_integrator[2]; PR->Output[2] = PR->output_of_forward_integrator[2] + PR->Kp * PR->error[2];
// }
// static inline float *ThreePhase_PRdiff_Calculate(PR_Diff_Typedef *PR_Diff[], float Target[3], float Actual[3])
// {
//     /*===================== 单次 PR 计算 =====================*/
//     float Ut[3] = {0.0f, 0.0f, 0.0f};
//     // 计算误差
//     float Xn = Target[0] - Actual[0];
//     float Yn = PR_Diff[0]->Coeff_B0 * Xn + PR_Diff[0]->Coeff_B1 * PR_Diff[0]->Xn_1 + PR_Diff[0]->Coeff_B2 *
//     PR_Diff[0]->Xn_2 -
//          PR_Diff[0]->Coeff_A1 * PR_Diff[0]->Yn_1 - PR_Diff[0]->Coeff_A2 * PR_Diff[0]->Yn_2;
//     // 更新历史缓冲区
//     PR_Diff[0]->Xn_2 = PR_Diff[0]->Xn_1; // Xn-2 = Xn-1
//     PR_Diff[0]->Xn_1 = Xn;               // Xn-1 = Xn
//     PR_Diff[0]->Yn_2 = PR_Diff[0]->Yn_1; // Yn-2 = Yn-1
//     Ut[0] = PR_Diff[0]->Yn_1 = Yn;               // Yn-1 = Yn

//     Xn = Target[1] - Actual[1];
//     Yn = PR_Diff[1]->Coeff_B0 * Xn + PR_Diff[1]->Coeff_B1 * PR_Diff[1]->Xn_1 + PR_Diff[1]->Coeff_B2 *
//     PR_Diff[1]->Xn_2 -
//          PR_Diff[1]->Coeff_A1 * PR_Diff[1]->Yn_1 - PR_Diff[1]->Coeff_A2 * PR_Diff[1]->Yn_2;
//     // 更新历史缓冲区
//     PR_Diff[1]->Xn_2 = PR_Diff[1]->Xn_1; // Xn-2 = Xn-1
//     PR_Diff[1]->Xn_1 = Xn;               // Xn-1 = Xn
//     PR_Diff[1]->Yn_2 = PR_Diff[1]->Yn_1; // Yn-2 = Yn-1
//     Ut[2] = PR_Diff[1]->Yn_1 = Yn;               // Yn-1 = Yn
//     Ut[1] = -Ut[2] -  Ut[0];

//     return Ut;
// }
/**
 * @brief 三相PR算法计算宏定义（单相）
 * @param PR 三相PR结构体指针
 * @param PR_Target 目标电压数组
 * @param PR_Actual 当前电压数组
 * @param phase 相位（0, 1, 2）
 */
#define PROCESS_PHASE(PR, PR_Target, PR_Actual, phase)                                                                 \
    do                                                                                                                 \
    {                                                                                                                  \
        (PR)->error[phase] = (PR_Target)[phase] - (PR_Actual)[phase];                                                  \
        (PR)->input_of_forward_integrator[phase] = (PR)->temp1 * (PR)->error[phase] - (PR)->output_of_feedback[phase]; \
        (PR)->output_of_forward_integrator[phase] += (PR)->ts * (PR)->input_of_forward_integrator[phase];              \
        (PR)->output_of_backward_integrator[phase] += (PR)->output_of_forward_integrator[phase] * (PR)->w0_2_ts;       \
        (PR)->output_of_feedback[phase] =                                                                              \
            (PR)->output_of_backward_integrator[phase] + (PR)->temp2 * (PR)->output_of_forward_integrator[phase];      \
        (PR)->Output[phase] = (PR)->output_of_forward_integrator[phase] + (PR)->Kp * (PR)->error[phase];               \
    } while (0)

/**
 * @brief 三相PR算法计算宏定义（三相）
 * @param PR 三相PR结构体指针
 * @param PR_Target 目标电压数组
 * @param PR_Actual 当前电压数组
 */
#define _THREE_PHASE_PR_CALCULATE(PR, PR_Target, PR_Actual)   \
    do                                                        \
    {                                                         \
        PROCESS_PHASE(PR, PR_Target, PR_Actual, 0); /* A相 */ \
        PROCESS_PHASE(PR, PR_Target, PR_Actual, 1); /* B相 */ \
        PROCESS_PHASE(PR, PR_Target, PR_Actual, 2); /* C相 */ \
    } while (0)

#endif /* INC_PR_H_ */
